

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<unistd.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/imgcodecs/legacy/constants_c.h>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<iomanip>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include<thread>
#include <list>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include  "include/stereo_camera_parameter.h"
using namespace cv;
using namespace std;
void LoadImages(const string &strFile, vector<string> &vstrImageFilenames,
                vector<double> &vTimestamps);
                
//depth mm                
void depthToPoints(
	const float * depthparam_param_after_rotate,
	const int max_depth_th,  const uint16_t * pDepthBuff, const int depth_width ,const int depth_height,
	cv::Point3f * pPoints, int & point_num)
{
	int pt_num = 0;
	float fx,fy,cx,cy;
	fx = depthparam_param_after_rotate[0];
	fy = depthparam_param_after_rotate[1];
	cx = depthparam_param_after_rotate[2];
	cy = depthparam_param_after_rotate[3];

	float max_depth_th_m = max_depth_th / 1000.f;
	for(int r_d=0;r_d < depth_height;r_d++){
		for(int c_d=0;c_d < depth_width;c_d++){
			float depth_now = pDepthBuff[r_d*depth_width+c_d]/1000.f;
			if(depth_now > max_depth_th_m || depth_now < 0.2){
				continue;
			}
			
			pPoints[pt_num].x = (c_d - cx)*depth_now/fx;
			pPoints[pt_num].y = (r_d - cy)*depth_now/fy;
			pPoints[pt_num].z = depth_now;
			
			pt_num++;
		}
	}
	point_num = pt_num;
					cout<<" pt_num "<<pt_num<<endl;

}

//rgb_instrinscs_ fx,fy,cx,cy,
//rgb_distdistortion: k0,k1,k2,k3
// rgbPoints: pixel_x, pixel_y, z_in_rgb_coor_m
void pointsToRgb(cv::Mat left_rgb_img, const cv::Point3f * pPoints, const int & point_num,
	const float * rgb_instrinscs_, const float *rgb_distortion,
	const cv::Mat & Tpoint2rgb, cv::Point3f * rgbPoints){
	//trans to rgb
	
	//trans to pixel
	float fx,fy,cx,cy;
	std::vector<cv::Point2f> undistorted;
	fx = rgb_instrinscs_[0]; fy = rgb_instrinscs_[1]; cx = rgb_instrinscs_[2]; cy = rgb_instrinscs_[3];
	for(int iN = 0; iN< point_num; iN++){

		undistorted.push_back(cv::Point2f(pPoints[iN].x / pPoints[iN].z, pPoints[iN].y / pPoints[iN].z));
	}
	
	
	//distort
	float k0,k1,k2,k3;
    k0 = rgb_distortion[0]; k1 = rgb_distortion[1]; k2 = rgb_distortion[2]; k3=rgb_distortion[3];

	std::vector<cv::Point2f> distorted;
	cv::Mat K = (cv::Mat_<double>(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
    cv::Mat D = (cv::Mat_<double>(1,4) << k0, k1, k2, k3);

    K.convertTo(K, CV_64FC1);
    D.convertTo(D, CV_64FC1);

	cv::fisheye::distortPoints(undistorted, distorted, K, D);

    for(int iN = 0; iN< point_num; iN++){
        rgbPoints[iN].x = distorted[iN].x;
        rgbPoints[iN].y = distorted[iN].y;
        rgbPoints[iN].z = pPoints[iN].z;
    }
	cout<<"------ K "<< K <<" D "<<D<<endl;
	
	//try undistort img 
	cv::Mat undistorted_img, newK;
    if(0){
        Mat newimage;
        Mat imageInput = left_rgb_img.clone();

        Mat mapx, mapy,K_new;
        cv::Size new_image_size=left_rgb_img.size();
        initUndistortRectifyMap(K,D,Mat(),K_new,new_image_size,CV_32FC1,mapx,mapy);
        remap(imageInput, newimage, mapx, mapy, CV_INTER_AREA, BORDER_CONSTANT, 255);
        cout<<" newimage "<<newimage.type()<<" "<< newimage.size()<<endl;
        imshow("newimage", newimage); waitKey(-1);
        imwrite("remap.jpg",newimage);
        imwrite("origin.jpg",imageInput);      


        new_image_size=cv::Size(left_rgb_img.cols*2, left_rgb_img.rows*2);
        initUndistortRectifyMap(K,D,Mat(),K_new,new_image_size,CV_32FC1,mapx,mapy);
        remap(imageInput, newimage, mapx, mapy, CV_INTER_AREA, BORDER_CONSTANT, 255);
        cout<<" newimage "<<newimage.type()<<" "<< newimage.size()<<endl;
        imshow("newimage", newimage); waitKey(-1);
        imwrite("remap_2.jpg",newimage);
        imwrite("origin_2.jpg",imageInput);
    }

    {
        Mat imageInput = left_rgb_img.clone();

        cv::Size new_image_size=left_rgb_img.size();
        cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K,D,new_image_size,
                                                                cv::Matx33d::eye(),newK);
        fisheye::undistortImage(left_rgb_img, undistorted_img, K, D, newK);
        cout<<"newK "<<newK<<endl;

        cout<<" newimage "<<undistorted_img.type()<<" "<< undistorted_img.size()<<endl;
        imshow("newimage", undistorted_img); waitKey(-1);
        imwrite("undistorted_img.jpg",undistorted_img);
        imwrite("origin.jpg",imageInput);

        cv::Mat rgbShow ;
        cv::cvtColor(undistorted_img, rgbShow, COLOR_GRAY2RGB);

        float fx_new = newK.at<double>(0,0);
        float fy_new = newK.at<double>(1,1);
        float cx_new = newK.at<double>(0,2);
        float cy_new = newK.at<double>(1,2);
        cout<<" newK: "<<newK<<endl;
        cout<<" fx_new "<<fx_new<<" fy_new "<<fy_new<<" cx_new "<<cx_new<<" cy_new "<<cy_new<<endl;
        //blue show depth
        for(int iN = 0; iN< point_num; iN++){
            float x0 = pPoints[iN].x;
            float y0 = pPoints[iN].y;
            float z0 = pPoints[iN].z;

            float u0 = x0 / z0 * fx_new + cx_new;
            float v0 = y0 / z0 * fy_new + cy_new;

            rgbShow.at<cv::Vec3b>(v0, u0) = cv::Vec3b(255,0,0);
        }
            imshow("undistort_leftir_with_point", rgbShow); waitKey(-1);
            imwrite("undistort_leftir_with_point.jpg",rgbShow);


	}

}

//rgb_instrinscs_ fx,fy,cx,cy,
//rgb_distdistortion: k0,k1,k2,k3
// rgbPoints: pixel_x, pixel_y, z_in_rgb_coor_m
void undistortImageNative(cv::Mat left_rgb_img,
    const float * rgb_instrinscs_, const float *rgb_distortion){
    //trans to rgb

    //trans to pixel
    float fx,fy,cx,cy;
    fx = rgb_instrinscs_[0]; fy = rgb_instrinscs_[1]; cx = rgb_instrinscs_[2]; cy = rgb_instrinscs_[3];



    //distort
    float k0,k1,k2,k3;
    k0 = rgb_distortion[0]; k1 = rgb_distortion[1]; k2 = rgb_distortion[2]; k3=rgb_distortion[3];

    std::vector<cv::Point2f> distorted;
    cv::Mat K = (cv::Mat_<double>(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
    cv::Mat D = (cv::Mat_<double>(1,4) << k0, k1, k2, k3);

    K.convertTo(K, CV_64FC1);
    D.convertTo(D, CV_64FC1);

    //cv::fisheye::distortPoints(undistorted, distorted, K, D);
    cout<<" K "<< K <<" D "<<D<<endl;

    //try undistort img
    cv::Mat undistorted_img, newK;
    fisheye::undistortImage(left_rgb_img, undistorted_img, K, D);
    cout<<" left_rgb_img "<<left_rgb_img.type()<<" "<< left_rgb_img.size()<<endl;

    cout<<" undistorted_img "<<undistorted_img.type()<<endl;
    imshow("undistorted_img", undistorted_img); waitKey(-1);

    {
        Mat newimage;
        Mat imageInput = left_rgb_img.clone();

        Mat mapx, mapy,K_new;
        cv::Size new_image_size=left_rgb_img.size();
        initUndistortRectifyMap(K,D,Mat(),K_new,new_image_size,CV_32FC1,mapx,mapy);
        remap(imageInput, newimage, mapx, mapy, CV_INTER_AREA, BORDER_CONSTANT, 255);
        cout<<" newimage "<<newimage.type()<<" "<< newimage.size()<<endl;
        imshow("newimage", newimage); waitKey(-1);
        imwrite("remap.jpg",newimage);
        imwrite("origin.jpg",imageInput);


        new_image_size=cv::Size(left_rgb_img.cols*2, left_rgb_img.rows*2);
        initUndistortRectifyMap(K,D,Mat(),K_new,new_image_size,CV_32FC1,mapx,mapy);
        remap(imageInput, newimage, mapx, mapy, CV_INTER_AREA, BORDER_CONSTANT, 255);
        cout<<" newimage "<<newimage.type()<<" "<< newimage.size()<<endl;
        imshow("newimage", newimage); waitKey(-1);
        imwrite("remap_2.jpg",newimage);
        imwrite("origin_2.jpg",imageInput);
    }

}



cv::Mat showPointOnRgb(cv::Mat left_img, const int depth_show_th_mm, const cv::Point3f * pPoints, const int & point_num){
	cv::Mat rgbShow ;
	cv::cvtColor(left_img, rgbShow, COLOR_GRAY2RGB);
	

	
	//blue show depth
	float show_depth_th_m = depth_show_th_mm / 1000.f;
	for(int iN = 0; iN< point_num; iN++){
		int ux = pPoints[iN].x;
		int uy = pPoints[iN].y;
		
		float weight = pPoints[iN].z / show_depth_th_m;
		weight = max(0.f, min(1.f, weight));
		
		//rgbShow.at<cv::Vec3b>(uy, ux)(2) = max(255, (int)(weight*255));
				rgbShow.at<cv::Vec3b>(uy, ux) = cv::Vec3b(255,0,0);

		
	}
		imshow("point_left_img", rgbShow); waitKey(10);
        imwrite("point_on_leftir.jpg", rgbShow);


}
void loadParameter(const std::string yamlfile, stereoCameraParametersNew &paras)
{
  cv::FileStorage fs(yamlfile,cv::FileStorage::READ);
  if(!fs.isOpened())
  {
    std::cerr<<"failed to open yaml file: "<<yamlfile<<std::endl;
    return;
  }

  //load
  paras.origin_width = (int)fs["irwidth"];
  paras.origin_height = (int)fs["irheight"];
  paras.undistortion_width = (int)fs["rgbwidth"];
  paras.udistortion_height = (int)fs["rgbheight"];

  //depth
  cv::FileNode depthparam = fs["depthparam"];
  int result = (int)depthparam["result"];
  std::vector<float> param;
  depthparam["param_after_rotate"] >> param;
  if(param.size()==5){
    std::copy(param.begin(), param.end(), paras.depthparam_param_after_rotate);
  }

  //left
  std::vector<float> left_fx_fy;
  fs["left_ir_focus"] >> left_fx_fy;
  std::vector<float> left_cx_cy;
  fs["left_ir_photocenter"] >> left_cx_cy;
  paras.left_instrinsics_undis[0]= left_fx_fy[0];
  paras.left_instrinsics_undis[1]= left_fx_fy[1];
  paras.left_instrinsics_undis[2]= left_cx_cy[0];
  paras.left_instrinsics_undis[3]= left_cx_cy[1];
  std::vector<float> left_distortion;
  fs["left_distortion"] >> left_distortion;
  if(left_distortion.size()==8){
    std::copy(left_distortion.begin(), left_distortion.end(), paras.left_instrinsics);
  }

  //right
  std::vector<float> right_fx_fy;
  fs["right_ir_focus"] >> right_fx_fy;
  std::vector<float> right_cx_cy;
  fs["right_ir_photocenter"] >> right_cx_cy;
  paras.right_instrinsics_undis[0]= right_fx_fy[0];
  paras.right_instrinsics_undis[1]= right_fx_fy[1];
  paras.right_instrinsics_undis[2]= right_cx_cy[0];
  paras.right_instrinsics_undis[3]= right_cx_cy[1];
  std::vector<float> right_distortion;
  fs["right_distortion"] >> right_distortion;
  if(right_distortion.size()==8){
    std::copy(right_distortion.begin(), right_distortion.end(), paras.right_instrinsics);
  }

  std::cout<<"right dis: ";
  for (int i =0 ; i < 8;i++){
     std::cout<<paras.right_instrinsics[i]<<" ";
  }
  std::cout<<std::endl;
 



}

string exit_fileName="/tmp/interruput_nv";
int main(int argc, char **argv)
{

    //instrincs_test(); return 1;


        //load yaml
        string yamlFile = string(argv[1])+"/camera_params.yml";
        cerr<<" yamlFile "<<yamlFile<<endl;


       stereoCameraParametersNew paras;
       loadParameter(yamlFile, paras);
       paras.undistortion_width = 640;
       paras.udistortion_height = 480;
      //  return 0;

        vector<string> vstrImageFilenames;
        vector<double> vTimestamps;//ms

        string strFile = string(argv[1])+"/left_ir.txt";
        LoadImages(strFile, vstrImageFilenames, vTimestamps);
        
        
        vector<string> vstrImageFilenames_right;
        vector<double> vTimestamps_right;//ms

        strFile = string(argv[1])+"/right_ir.txt";
        LoadImages(strFile, vstrImageFilenames_right, vTimestamps_right);
        
        
        vector<string> vstrDepthFilenames;
        vector<double> vDepthTimestamps;//ms

        string strDepthFile = string(argv[1])+"/depth.txt";
        LoadImages(strDepthFile, vstrDepthFilenames, vDepthTimestamps);
        
        cout<<" depth file num "<< vstrDepthFilenames.size()<<" rgb file size "<< vstrImageFilenames.size()<<endl;
        
        int width = paras.origin_width;
        int height= paras.origin_height;
        // float rgb_instrinscs[]={380.977051, 380.876495, 518.386719, 337.40976};
        float rgb_instrinscs[] ={ paras.left_instrinsics[4],paras.left_instrinsics[5],paras.left_instrinsics[6],paras.left_instrinsics[7]};
        float rgb_distortion[] ={ paras.left_instrinsics[0],paras.left_instrinsics[1],paras.left_instrinsics[2],paras.left_instrinsics[3]};
        
	    int yuv_byte_num = width*height*1.5;
		uchar * temp_buff =(uchar*)malloc(yuv_byte_num);		
		int depth_width = paras.undistortion_width;
		int depth_height = paras.udistortion_height;
		uint16_t  pDepthBuff[depth_width*depth_height];//mm
		// float depthparam_param_after_rotate[] = {161.301956, 161.301956, 334.934448, 201.649963, -54.1767044};
    float* depthparam_param_after_rotate = paras.depthparam_param_after_rotate;
        int matched_depth_idx = 0;
        double depth_diff_rgb_max_ms = 0;
        for(int ni=0; 0&&ni<vstrImageFilenames_right.size(); ni++)
        {
            string img_name = string(argv[1])+"/"+vstrImageFilenames_right[ni];
            //im = cv::imread(img_name,CV_LOAD_IMAGE_UNCHANGED);
            //cout<<" try open file "    <<img_name<<endl;
            FILE *fp = fopen(img_name.c_str(),"rb");
            if(fp==NULL)
            {
                cout<<" can't open file "    <<img_name<<endl;
                continue;
            }
            cv::Mat fisheye_image = imread(img_name,CV_LOAD_IMAGE_UNCHANGED),fisheye_image_gray;
            if(fisheye_image.empty())
            {
				size_t read_size = fread(temp_buff, 1, yuv_byte_num, fp);
				fclose(fp);
				if(read_size == yuv_byte_num)
				{
					fisheye_image = cv::Mat(height+height/2, width, CV_8UC1, temp_buff);
					fisheye_image_gray = cv::Mat(height, width, CV_8UC1, temp_buff);

          cv::Mat fisheye_image2;
					cv::cvtColor(fisheye_image, fisheye_image2, COLOR_YUV2BGR_NV12);
					
					// imwrite(img_name, fisheye_image2);	
          // imshow("rigth rgb", fisheye_image2);
          // waitKey(-1);				
				}
				else
				{
					cout<<img_name<<" read_size "<<read_size<<" "<<yuv_byte_num<<endl;
					continue;
				}
			}
			imshow("right_fisheye_image_gray ",fisheye_image_gray);
            waitKey(10);
        }
        for(int ni=0; ni<vstrImageFilenames.size(); ni++)
        {
            string img_name = string(argv[1])+"/"+vstrImageFilenames[ni];
            //im = cv::imread(img_name,CV_LOAD_IMAGE_UNCHANGED);
            //cout<<" try open file "    <<img_name<<endl;
            FILE *fp = fopen(img_name.c_str(),"rb");
            if(fp==NULL)
            {
                cout<<" can't open file "    <<img_name<<endl;
                continue;
            }
            cv::Mat fisheye_image = imread(img_name,CV_LOAD_IMAGE_UNCHANGED),fisheye_image_gray;
            if(fisheye_image.empty())
            {
				size_t read_size = fread(temp_buff, 1, yuv_byte_num, fp);
				fclose(fp);
				if(read_size == yuv_byte_num)
				{
					fisheye_image = cv::Mat(height+height/2, width, CV_8UC1, temp_buff);
					fisheye_image_gray = cv::Mat(height, width, CV_8UC1, temp_buff);
					//cv::cvtColor(fisheye_image, fisheye_image2, COLOR_YUV2BGR_NV12);
					
					//imwrite(img_name, fisheye_image);					
				}
				else
				{
					cout<<img_name<<" read_size "<<read_size<<" "<<yuv_byte_num<<endl;
					continue;
				}
							//cout<<img_name<<" read_size "<<read_size<<" "<<yuv_byte_num<<endl;

			}
			imshow("left_fisheye_image_gray ",fisheye_image_gray);
            waitKey(10);
			//cout<<img_name<<" "<<(float)((float)ni/(float)vstrImageFilenames.size())<<endl;
			if(access(exit_fileName.c_str(),F_OK)==0)
			{
				cerr<<" exit_fileName "<<exit_fileName<<" checked, finish  "<<endl;
				return 0;
			}
            //undistortImageNative(fisheye_image_gray, rgb_instrinscs, rgb_distortion);
						
			//if get rgb ok, try depth show
			double t_delta = 1000;
			#if 1
			while(matched_depth_idx < vstrDepthFilenames.size())//1ms
			{
				t_delta = fabs(vDepthTimestamps[matched_depth_idx]-vTimestamps[ni]);
				if(t_delta < 30){
					cout<<fixed<<setprecision(5)<<" matched_depth_idx: "<<matched_depth_idx<<" t_delta "<<t_delta<<" vTimestamps[i] "<<vTimestamps[ni]<<" vstrDepthFilenames[matched_depth_idx] "<< vstrDepthFilenames[matched_depth_idx]<<endl;
					break;
				}	
				//cout<<fixed<<setprecision(5)<<" matched_depth_idx: "<<matched_depth_idx<<" t_delta "<<t_delta<<" vTimestamps[i] "<<vTimestamps[ni]<<" vstrDepthFilenames[matched_depth_idx] "<< vstrDepthFilenames[matched_depth_idx]<<endl;

				matched_depth_idx++;
			}

			if(t_delta<30){
        std::cout << "t_delta: " << t_delta << std::endl;
				//show depth
	            string img_name_depth = string(argv[1])+"/"+vstrDepthFilenames[matched_depth_idx];
				FILE *fp_d = fopen(img_name_depth.c_str(),"rb");
				if(fp_d==NULL)
				{
					cout<<" can't open file "    <<img_name_depth<<endl;
					continue;
				}
									//cout<<" open file "    <<img_name_depth <<" pDepthBuff "<<sizeof(pDepthBuff)<<endl;
				size_t read_size_depth = fread(pDepthBuff, 1, depth_width*depth_height*2, fp_d);									cout<<" open file "    <<img_name_depth<<endl;
									//cout<<" open file "    <<img_name_depth<<" read_size_depth "<<read_size_depth <<endl;
				fclose(fp_d);
				if(read_size_depth != depth_width*depth_height*2){
					cout<<" read depth raw file failed read_size_depth "<<read_size_depth<<endl;
					continue;
				}
				cv::Mat depth_image = cv::Mat(depth_height, depth_width, CV_16UC1, pDepthBuff);
				cv::Mat showDepth(depth_height, depth_width, CV_8UC1);
				//max min value
				uint16_t max_d = 0, min_d = 65536;
				for(int r_d=0;r_d < depth_height;r_d++){
					for(int c_d=0;c_d < depth_width;c_d++){
						max_d = max(pDepthBuff[r_d*depth_width+c_d], max_d);
						min_d = min(pDepthBuff[r_d*depth_width+c_d], min_d);
						float depth_now = pDepthBuff[r_d*depth_width+c_d]/1000.f;
						if(depth_now < 2 && depth_now > 0.2){
							;//cout<<" r_d "<<r_d<<" c_d "<<c_d<<" depth "<< depth_now<<endl;
						}
						int value_gray = depth_now*255/5;//5m->255
						showDepth.data[r_d*depth_width+c_d]=min(255, max(value_gray, 0));
					}
				}
				//cout<<" max_d "<<max_d<<" min_d "<<min_d<<endl;
				depth_diff_rgb_max_ms = max(depth_diff_rgb_max_ms, t_delta);
				imshow("showDepth",showDepth);waitKey(1);
				cv::Point3f points[depth_width*depth_height];
				int points_num =0;
				depthToPoints(depthparam_param_after_rotate, 7000, pDepthBuff, depth_width, depth_height, points, points_num);
				
				cv::Point3f rgbPoints[points_num];
				pointsToRgb(fisheye_image_gray, points, points_num, rgb_instrinscs, rgb_distortion, cv::Mat(), rgbPoints);
				
				cout<< "points_num "<<points_num<<endl;
				/*show on rgb-left*/
				showPointOnRgb(fisheye_image_gray, 7000, rgbPoints, points_num);
			}
			else
				cout<<fixed<<setprecision(5)<<" failed matched_depth_idx: "<<matched_depth_idx<<" t_delta "<<t_delta<<" vTimestamps[i] "<<vTimestamps[ni]<<endl;
		#endif

    }
    cout<<" depth_diff_rgb_max_ms "<<depth_diff_rgb_max_ms<<endl;
    return 0;
}

void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
    ifstream f;
    f.open(strFile.c_str());
    const double std_interval_ms = 100;
    double diff_thresh_ms = 10;

    double timestamp_last = -1;
    while(!f.eof())
    {
        string s;
        getline(f,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            int t1, t2;
            double timestamp;
            string sRGB;
            ss >> timestamp;
            vTimestamps.push_back(timestamp);
            ss >> t1>>t2;
            ss >> sRGB;
            vstrImageFilenames.push_back(sRGB);
            if(timestamp_last>0){
				double diff0 = timestamp - timestamp_last;
				double error0 = fabs(diff0-std_interval_ms);
				if(error0 >=  diff_thresh_ms){
					cout<<fixed<<setprecision(5)<<" rgb diff big timestamp "<<timestamp<<" diff0 "<<diff0<<endl;

				}
				
				
			}
			timestamp_last = timestamp;
            //cout<<" timestamp "<<timestamp<<" "<<t1<<" "<<t2<<" img_name: "<<sRGB<<endl;
        }
    }
}
#if 0
typedef struct {
  unsigned char r;
  unsigned char g;
  unsigned char b;
}MYRGB;
MYRGB * m_colortable=NULL;
static int minDepth = 0;
static int maxDepth = 10;
int nvpfm_getyuvfromindex(int index, unsigned char *py, unsigned char *pu, unsigned char *pv) {
  if (index < 255 && index >= 0) {
    *py = m_colortable[index].r;
    *pu = m_colortable[index].g;
    *pv = m_colortable[index].b;
    return 0;
  } else {
    return -1;
  }
}

void calculatecolortable() {
  if (NULL == m_colortable) {
    m_colortable = (MYRGB *)calloc(1, sizeof(MYRGB) * 65536);
  }
  memset(m_colortable, 0, sizeof(MYRGB) * 65536);

  for (int i = (int)minDepth; i < 65536; i++) { // 0.2m-5m
    if (i > (int)maxDepth) {
      unsigned char y = 0;
      unsigned char u = 0;
      unsigned char v = 0;
      int result = nvpfm_getyuvfromindex(254, &y, &u, &v);
      float fr = y + 1.4075 * (v - 128);

      float fg = y - 0.3455 * (u - 128) - 0.7169 * (v - 128);

      float fb = y + 1.779 * (u - 128);
      /*		static FILE* fp = fopen("table.txt", "wt");
      fprintf(fp,"table:%f,%f,%f\n", fr, fg, fb);
      fflush(fp);
      static FILE* fp = fopen("table.txt", "wt");
      fprintf(fp, ">max,:%f,%f,%f\n", fr, fg, fb);
      fflush(fp);*/
      m_colortable[i].r = (unsigned char)fr;
      m_colortable[i].g = (unsigned char)fg;
      m_colortable[i].b = (unsigned char)fb;
      continue;
    }
    int effectindex = (int)((float)(i - (int)minDepth) * 255.0 / (float)(maxDepth - minDepth));
    //	if (effectindex > 255)effectindex = 255;
    if (effectindex == 255)
      effectindex = 254;

    unsigned char y = 0;
    unsigned char u = 0;
    unsigned char v = 0;
    int result = nvpfm_getyuvfromindex(effectindex, &y, &u, &v);
    // y = 26; u = 170; v = 122;
    if (result >= 0) {
      float fr = y + 1.4075 * (v - 128);

      float fg = y - 0.3455 * (u - 128) - 0.7169 * (v - 128);

      float fb = y + 1.779 * (u - 128);
      /*		static FILE* fp = fopen("table.txt", "wt");
      fprintf(fp, "index:%d,:%f,%f,%f\n", effectindex, fr, fg, fb);
      fflush(fp);
      */
      m_colortable[i].r = (unsigned char)fr;
      m_colortable[i].g = (unsigned char)fg;
      m_colortable[i].b = (unsigned char)fb;
    }
  }
}
void compute_depth2pseudo(uint16_t *depthdata, int width, int height, uint8_t *pseudo, uint16_t max_d, uint16_t min_d, bool rgborbgr) {
  if (max_d == 0 && min_d == 0) {
    if (m_colortable == NULL) {
      calculatecolortable();
    }
  } else {
    if (m_colortable == NULL || (maxDepth != max_d || minDepth != min_d)) {
      maxDepth = max_d;
      minDepth = min_d;
      calculatecolortable();
    }
  }

  for (int row = 0; row < height; row++) {
    for (int col = 0; col < width; col++) {
      int index = row * width + col;
      uint16_t distance = *(depthdata + row * width + col);
      MYRGB *color = (m_colortable + distance);

      pseudo[index * 3] = (rgborbgr ? color->r : color->b);     // info->cr;
      pseudo[index * 3 + 1] = color->g;                         // info->cg;
      pseudo[index * 3 + 2] = (rgborbgr ? color->b : color->r); // info->cb;
    }
  }
}
#endif

